import minimalmodbus as modbus
import keyboard
import time
import redis
import json
import math
import random


class Wheel(object):
    def __init__(self, _port, _slaveaddress, p_setting_speed, p_setting_direction, p_forward_direction,
                 p_backward_direction, function_code):
        self.slave = modbus.Instrument(port=_port, slaveaddress=_slaveaddress)
        self.slave.debug = False
        self.slave.serial.timeout = 0.1
        self.slave.serial.baudrate = 9600
        self.function_code = function_code
        # self.slave.write_register(0x00B6, 0x0001, 0, self.function_code)
        self.slave.write_register(0x4657, 1, 0, self.function_code)

        self.p_setting_speed = p_setting_speed
        self.p_setting_direction = p_setting_direction
        self.p_forward_direction = p_forward_direction
        self.p_backward_direction = p_backward_direction

        self.current_speed = 0

        self.is_forward = False
        self.is_backward = False

        self.scale = 400

    def forward(self, running_speed=0):
        if running_speed == 0:
            self.current_speed = self.current_speed + self.scale
        else:
            self.current_speed = running_speed

        if not self.is_forward:
            self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)
            self.slave.write_register(self.p_setting_direction, self.p_forward_direction, 0, self.function_code)
            self.is_forward = True
        else:
            self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)

    def slow_down(self):
        self.current_speed = self.current_speed - self.scale * 2
        if self.current_speed < 0:
            self.current_speed = 0

        if self.is_forward or self.is_backward:
            self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)

        if self.current_speed == 0:
            self.is_forward = False
            self.is_backward = False

    def stop(self):
        self.is_forward = False
        self.is_backward = False
        self.current_speed = 0

        self.slave.write_register(self.p_setting_speed, 0, 0, self.function_code)

    def backward(self, running_speed=0):
        if running_speed == 0:
            self.current_speed = self.current_speed + self.scale
        else:
            self.current_speed = running_speed

        if not self.is_forward:
            self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)
            self.slave.write_register(self.p_setting_direction, self.p_backward_direction, 0, self.function_code)
            self.is_backward = True
        else:
            self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)


class Drive(object):
    def __init__(self, left, right):
        self.left = left
        self.right = right
        self.r = redis.Redis(db=0)
        self.speed = 0
        self.new_job = False

        self.task_index = 1
        self.last_oy = 0

    def forward_speed_up(self):
        if self.left.is_backward or self.right.is_backward:
            self.speed_down()
        else:
            self.left.forward()
            self.right.forward()

    def forward_constant_speed(self):
        print('forward')
        self.left.forward(self.speed)
        self.right.forward(self.speed)

    def speed_down(self):
        self.left.slow_down()
        self.right.slow_down()

    def go_right(self):
        if self.left.current_speed < self.right.current_speed:
            if self.left.is_forward or self.right.is_forward:
                self.left.forward()
            else:
                self.left.backward()
        else:
            self.right.slow_down()

    def go_left(self):
        if self.right.current_speed < self.left.current_speed:
            if self.left.is_forward or self.right.is_forward:
                self.right.forward()
            else:
                self.right.backward()
        else:
            self.left.slow_down()

    def backward_speed_up(self):
        if self.left.is_forward or self.right.is_forward:
            self.speed_down()
        else:
            self.left.backward()
            self.right.backward()

    def rotate_right(self):
        self.left.forward()
        self.right.backward()

    def rotate_left(self):
        self.left.backward()
        self.right.forward()

    def stop(self):
        self.left.stop()
        self.right.stop()

    def programed_walk(self):
        pass

    def auto_straight_left(self, diff):
        print('left')
        if diff > 150*7:
            diff = 150*7
        self.left.forward(self.speed - diff)
        self.right.forward(self.speed)

    def auto_straight_right(self, diff):
        print('right')
        if diff > 150*7:
            diff = 150*7
        self.left.forward(self.speed)
        self.right.forward(self.speed - diff)

    def auto_straight_forward_right_camera(self, constant_distance):
        if self.new_job:
            self.r.set('camera_1_command', 'off')
            self.r.set('camera_2_command', 'off')
            self.r.set('post_detect_command', 'off')
            time.sleep(1)
            self.r.set('camera_2_command', 'on')
            self.r.set('post_detect_command', 'on')
            time.sleep(17)

            self.new_job = False
            self.last_oy = 0

        coordinates = json.loads(self.r.get('coordinates'))

        coordinates = sorted(coordinates, key=lambda x: -x[3])
        print(coordinates)

        # for i in range(len(coordinates)):
        #     coordinates[i][1] = coordinates[i][1] + (coordinates[i][3] - 960)**2/1400
        coordinates = [x for x in coordinates if x[3] < 1800 and x[3] > 300]

        print(coordinates)

        buffer = 20
        diff = 80*9

        tx = float(self.r.get('tx'))
        ty = float(self.r.get('ty'))
        tz = float(self.r.get('tz'))

        distance = math.sqrt(tx**2 + ty**2 + tz**2)
        print(distance)

        if distance < 6000:
            self.speed = 1400*6
        else:
            self.speed = 1400*7

        if distance >= 26000:
            self.stop()
            self.task_index = self.task_index + 1
            self.new_job = True
            self.last_oy = 0

        elif len(coordinates) > 1:
            tail = coordinates[0]
            target = coordinates[-1]

            if target[1] > constant_distance:
                if target[1] < tail[1] - 300:
                    if target[1] < constant_distance + buffer:
                        self.auto_straight_left(diff)
                    else:
                        self.forward_constant_speed()
                else:
                    # self.auto_straight_right(abs(target[1] + tail[1] - constant_distance*2)/3)
                    self.auto_straight_left(diff)
            elif target[1] < constant_distance:
                if target[1] > tail[1] + 300:
                    if target[1] > constant_distance - buffer:
                        self.auto_straight_right(diff)
                    else:
                        self.forward_constant_speed()
                else:
                    # self.auto_straight_left(abs(target[1] + tail[1] - constant_distance*2)/3)
                    self.auto_straight_right(diff)
            else:
                self.forward_constant_speed()

        elif len(coordinates) == 1:

            target = coordinates[0]

            if target[1] > constant_distance + buffer:
                self.auto_straight_right(diff)
            elif target[1] < constant_distance - buffer:
                self.auto_straight_left(diff)
            else:
                self.forward_constant_speed()
        elif len(coordinates) == 0:
            self.forward_constant_speed()

        time.sleep(0.3)

    def auto_straight_forward_left_camera(self, constant_distance):
        if self.new_job:
            self.r.set('camera_1_command', 'off')
            self.r.set('camera_2_command', 'off')
            self.r.set('post_detect_command', 'off')
            time.sleep(0.4)
            self.r.set('camera_1_command', 'on')
            self.r.set('post_detect_command', 'on')
            time.sleep(13)

            self.new_job = False
            self.last_oy = 0

        coordinates = json.loads(self.r.get('coordinates'))

        coordinates = sorted(coordinates, key=lambda x: -x[3])
        print(coordinates)

        # for i in range(len(coordinates)):
        #     coordinates[i][1] = coordinates[i][1] + (coordinates[i][3] - 960)**2/1400
        coordinates = [x for x in coordinates if x[3] < 1300 and x[3] > 700]

        print(coordinates)

        buffer = 40
        diff = 80*7

        tx = float(self.r.get('tx'))
        ty = float(self.r.get('ty'))
        tz = float(self.r.get('tz'))

        distance = math.sqrt(tx**2 + ty**2 + tz**2)
        print(distance)

        if distance < 6000:
            self.speed = 1400*6
        else:
            self.speed = 1400*7

        if distance >= 26000:
            self.stop()
            self.task_index = self.task_index + 1
            self.new_job = True
            self.last_oy = 0
        elif len(coordinates) > 1:
            target = coordinates[0]
            tail = coordinates[-1]

            if target[1] > constant_distance:
                if target[1] < tail[1] - 300:
                    if target[1] < constant_distance + buffer:
                        self.auto_straight_right(diff)
                    else:
                        self.forward_constant_speed()
                else:
                    # self.auto_straight_left(abs(target[1] + tail[1] - constant_distance*2)/3)
                    self.auto_straight_left(diff)
            elif target[1] < constant_distance:
                if target[1] > tail[1] + 300:
                    if target[1] > constant_distance - buffer:
                        self.auto_straight_left(diff)
                    else:
                        self.forward_constant_speed()
                else:
                    # self.auto_straight_right(abs(target[1] + tail[1] - constant_distance*2)/3)
                    self.auto_straight_right(diff)
            else:
                self.forward_constant_speed()

        elif len(coordinates) == 1:
            target = coordinates[0]

            if target[1] > constant_distance + buffer:
                self.auto_straight_left(diff)
            elif target[1] < constant_distance - buffer:
                self.auto_straight_right(diff)
            else:
                self.forward_constant_speed()

        elif len(coordinates) == 0:
            self.forward_constant_speed()

        time.sleep(0.3)

    def left_2_lane(self, *argv):
        if self.new_job:
            self.r.set('camera_1_command', 'off')
            self.r.set('camera_2_command', 'off')
            self.r.set('post_detect_command', 'off')
            time.sleep(0.4)
            self.r.set('camera_1_command', 'on')

            self.new_job = False
            self.last_oy = 0

        oy = float(self.r.get('oy'))

        if abs(self.last_oy - oy) > 1.5:
            self.stop()
            self.task_index = self.task_index + 1
            self.new_job = True
            self.last_oy = 0
        else:
            self.left.forward(6000)
            self.right.forward(12300)
            self.last_oy = oy
            time.sleep(0.2)

    def auto_task(self):
        self.task_index = self.task_index % 4

        self.r.set('auto_task_status', 'running')

        task_list = [self.auto_straight_forward_left_camera, self.left_2_lane, self.auto_straight_forward_right_camera, self.left_2_lane]

        task_list_param = [5100, 0, 800, 0]

        if self.task_index < len(task_list):
            task_list[self.task_index](task_list_param[self.task_index])


if __name__ == '__main__':
    print('hello')
    # left = Wheel('/dev/ttyUSB0', 1, 0x0056, 0x0066, 2, 1, 6)
    # right = Wheel('/dev/ttyUSB0', 2, 0x0056, 0x0066, 1, 2, 6)

    left = Wheel('/dev/ttyUSB0', 2, 0x4420, 0x466D, 1, 0, 16)
    right = Wheel('/dev/ttyUSB0', 1, 0x4420, 0x466D, 0, 1, 16)

    autopilot = Drive(left, right)

    auto = False

    while True:
        try:
            if keyboard.is_pressed('i'):
                print('i')
                auto = False
                autopilot.forward_speed_up()
            elif keyboard.is_pressed('j'):
                print('j')
                auto = False
                autopilot.go_left()
            elif keyboard.is_pressed('k'):
                print('k')
                auto = False
                autopilot.backward_speed_up()
            elif keyboard.is_pressed('l'):
                print('l')
                auto = False
                autopilot.go_right()
            elif keyboard.is_pressed('o'):
                print('o')
                auto = False
                autopilot.rotate_right()
            elif keyboard.is_pressed('u'):
                print('u')
                auto = False
                autopilot.rotate_left()
            elif keyboard.is_pressed('space'):
                print('stop')
                auto = False
                autopilot.stop()
            elif keyboard.is_pressed('a'):
                print('a')
                autopilot.new_job = True
                auto = True
            elif keyboard.is_pressed('2'):
                print('2')
                autopilot.stop()
            elif auto:
                autopilot.auto_task()

            time.sleep(0.08)
        except modbus.NoResponseError:
            print('No response')
        

